#include "RVOSimulator.h"

#ifdef _OPENMP
#include <omp.h>
#endif

#include "Agent.h"
#include "KdTree.h"

namespace RVO
{
	// 用于初始化仿真器对象的成员变量，并分配KdTree对象的内存。
	RVOSimulator::RVOSimulator() : defaultAgent_(NULL), kdTree_(NULL), globalTime_(0.0f), timeStep_(0.0f)
	{
		kdTree_ = new KdTree(this); // 分配内存给kdTree_并将其初始化为新的KdTree对象，传入当前仿真器对象的指针 
	}

	// 用于根据传入的参数初始化仿真器对象及其默认智能体。
	RVOSimulator::RVOSimulator(float timeStep, float neighborDist, size_t maxNeighbors, float timeHorizon, float radius, float maxSpeed, const Vector3 &velocity)
	    : defaultAgent_(NULL), kdTree_(NULL), globalTime_(0.0f), timeStep_(timeStep)
	{
		kdTree_ = new KdTree(this);	 // 分配内存给kdTree_并将其初始化为新的KdTree对象，传入当前仿真器对象的指针
		defaultAgent_ = new Agent(this); // 分配内存给defaultAgent_并将其初始化为新的Agent对象，传入当前仿真器对象的指针

		// 初始化defaultAgent_的成员变量
		defaultAgent_->maxNeighbors_ = maxNeighbors; // 设置智能体的最大邻居数
		defaultAgent_->maxSpeed_ = maxSpeed;	     // 设置智能体的最大速度
		defaultAgent_->neighborDist_ = neighborDist; // 设置智能体的邻居距离
		defaultAgent_->radius_ = radius;	     // 设置智能体的半径
		defaultAgent_->timeHorizon_ = timeHorizon;   // 设置智能体的时间视野
		defaultAgent_->velocity_ = velocity;	     // 设置智能体的初始速度
	}

	// 用于释放动态分配的内存资源，确保程序不会出现内存泄漏。
	RVOSimulator::~RVOSimulator()
	{
		if (defaultAgent_ != NULL)
		{
			delete defaultAgent_; // 释放defaultAgent_指向的内存
		}

		for (size_t i = 0; i < agents_.size(); ++i)
		{
			delete agents_[i]; // 释放agents_向量中每个元素指向的内存
		}

		if (kdTree_ != NULL)
		{
			delete kdTree_; // 释放kdTree_指向的内存
		}
	}

	// 用于获取与智能体相关的信息，如邻居智能体数量、特定邻居的ID，以及ORCA平面的数量。
	size_t RVOSimulator::getAgentNumAgentNeighbors(size_t agentNo) const
	{
		return agents_[agentNo]->agentNeighbors_.size();
	}

	size_t RVOSimulator::getAgentAgentNeighbor(size_t agentNo, size_t neighborNo) const
	{
		return agents_[agentNo]->agentNeighbors_[neighborNo].second->id_;
	}

	size_t RVOSimulator::getAgentNumORCAPlanes(size_t agentNo) const
	{
		return agents_[agentNo]->orcaPlanes_.size();
	}

	// 用于获取指定智能体的特定ORCA平面。
	const Plane &RVOSimulator::getAgentORCAPlane(size_t agentNo, size_t planeNo) const
	{
		return agents_[agentNo]->orcaPlanes_[planeNo];
	}

	// 用于从仿真器中移除一个智能体。
	void RVOSimulator::removeAgent(size_t agentNo)
	{
		delete agents_[agentNo];	   // 释放指定智能体的内存
		agents_[agentNo] = agents_.back(); // 将最后一个智能体移到被删除智能体的位置
		agents_.pop_back();		   // 移除最后一个智能体
	}

	// 用于向仿真器中添加一个新的智能体，并初始化其属性。
	size_t RVOSimulator::addAgent(const Vector3 &position)
	{
		if (defaultAgent_ == NULL)
		{
			return RVO_ERROR; // 如果defaultAgent_为空，返回错误码RVO_ERROR
		}

		Agent *agent = new Agent(this); // 创建一个新的Agent对象，传入当前仿真器对象的指针

		// 初始化新智能体的属性
		agent->position_ = position;			     // 设置智能体的位置
		agent->maxNeighbors_ = defaultAgent_->maxNeighbors_; // 继承默认智能体的最大邻居数
		agent->maxSpeed_ = defaultAgent_->maxSpeed_;	     // 继承默认智能体的最大速度
		agent->neighborDist_ = defaultAgent_->neighborDist_; // 继承默认智能体的邻居距离
		agent->radius_ = defaultAgent_->radius_;	     // 继承默认智能体的半径
		agent->timeHorizon_ = defaultAgent_->timeHorizon_;   // 继承默认智能体的时间视野
		agent->velocity_ = defaultAgent_->velocity_;	     // 继承默认智能体的初始速度

		agent->id_ = agents_.size(); // 设置智能体的ID为当前智能体列表的大小

		agents_.push_back(agent); // 将新智能体添加到智能体列表中

		return agents_.size() - 1; // 返回新智能体的索引
	}

	// 用于向仿真器中添加一个新的智能体，并初始化其属性。
	size_t RVOSimulator::addAgent(const Vector3 &position, const Vector3 &position_base)
	{
		if (defaultAgent_ == NULL)
		{
			return RVO_ERROR; // 如果defaultAgent_为空，返回错误码RVO_ERROR
		}

		Agent *agent = new Agent(this); // 创建一个新的Agent对象，传入当前仿真器对象的指针

		// 初始化新智能体的属性
		// Vector3 relative_position_; // other->position_ - position_
		agent->position_ = position;			     // 设置智能体的位置
		agent->relative_position_ = position - position_base;
		agent->maxNeighbors_ = defaultAgent_->maxNeighbors_; // 继承默认智能体的最大邻居数
		agent->maxSpeed_ = defaultAgent_->maxSpeed_;	     // 继承默认智能体的最大速度
		agent->neighborDist_ = defaultAgent_->neighborDist_; // 继承默认智能体的邻居距离
		agent->radius_ = defaultAgent_->radius_;	     // 继承默认智能体的半径
		agent->timeHorizon_ = defaultAgent_->timeHorizon_;   // 继承默认智能体的时间视野
		agent->velocity_ = defaultAgent_->velocity_;	     // 继承默认智能体的初始速度

		agent->id_ = agents_.size(); // 设置智能体的ID为当前智能体列表的大小

		agents_.push_back(agent); // 将新智能体添加到智能体列表中

		return agents_.size() - 1; // 返回新智能体的索引
	}

	// 用于根据传入的参数创建和初始化一个新的智能体，并将其添加到仿真器中。
	size_t RVOSimulator::addAgent(const Vector3 &position, float neighborDist, size_t maxNeighbors, float timeHorizon, float radius, float maxSpeed, const Vector3 &velocity)
	{
		Agent *agent = new Agent(this); // 创建一个新的Agent对象，传入当前仿真器对象的指针

		// 初始化新智能体的属性
		agent->position_ = position;	     // 设置智能体的位置
		agent->maxNeighbors_ = maxNeighbors; // 设置智能体的最大邻居数
		agent->maxSpeed_ = maxSpeed;	     // 设置智能体的最大速度
		agent->neighborDist_ = neighborDist; // 设置智能体的邻居距离
		agent->radius_ = radius;	     // 设置智能体的半径
		agent->timeHorizon_ = timeHorizon;   // 设置智能体的时间视野
		agent->velocity_ = velocity;	     // 设置智能体的初始速度

		agent->id_ = agents_.size(); // 设置智能体的ID为当前智能体列表的大小

		agents_.push_back(agent); // 将新智能体添加到智能体列表中

		return agents_.size() - 1; // 返回新智能体的索引
	}

	// 用于执行仿真器的一个时间步。
	void RVOSimulator::doStep()
	{
		kdTree_->buildAgentTree(); // 构建或更新KD树，以便进行高效的邻居搜索

#ifdef _OPENMP
#pragma omp parallel for
#endif
		for (int i = 0; i < static_cast<int>(agents_.size()); ++i)
		{
			agents_[i]->computeNeighbors();	  // 计算每个智能体的邻居
			agents_[i]->computeNewVelocity(); // 计算每个智能体的新速度
		}

#ifdef _OPENMP
#pragma omp parallel for
#endif
		for (int i = 0; i < static_cast<int>(agents_.size()); ++i)
		{
			agents_[i]->update(); // 更新每个智能体的位置和速度
		}

		globalTime_ += timeStep_; // 增加全局时间
	}

	// 返回指定智能体的最大邻居数量。
	size_t RVOSimulator::getAgentMaxNeighbors(size_t agentNo) const
	{
		return agents_[agentNo]->maxNeighbors_;
	}

	// 返回指定智能体的最大速度。
	float RVOSimulator::getAgentMaxSpeed(size_t agentNo) const
	{
		return agents_[agentNo]->maxSpeed_;
	}

	// 返回指定智能体的邻居距离。
	float RVOSimulator::getAgentNeighborDist(size_t agentNo) const
	{
		return agents_[agentNo]->neighborDist_;
	}

	// 返回指定智能体的位置。
	const Vector3 &RVOSimulator::getAgentPosition(size_t agentNo) const
	{
		return agents_[agentNo]->position_;
	}

	// 返回指定智能体的偏好速度。
	const Vector3 &RVOSimulator::getAgentPrefVelocity(size_t agentNo) const
	{
		return agents_[agentNo]->prefVelocity_;
	}

	// 返回指定智能体的半径。
	float RVOSimulator::getAgentRadius(size_t agentNo) const
	{
		return agents_[agentNo]->radius_;
	}

	// 返回指定智能体的时间视野。
	float RVOSimulator::getAgentTimeHorizon(size_t agentNo) const
	{
		return agents_[agentNo]->timeHorizon_;
	}

	// 返回指定智能体的速度。
	const Vector3 &RVOSimulator::getAgentVelocity(size_t agentNo) const
	{
		return agents_[agentNo]->velocity_;
	}

	// 返回仿真器的全局时间。
	float RVOSimulator::getGlobalTime() const
	{
		return globalTime_;
	}

	// 返回仿真器中的智能体数量。
	size_t RVOSimulator::getNumAgents() const
	{
		return agents_.size();
	}

	// 返回仿真器的时间步长。
	float RVOSimulator::getTimeStep() const
	{
		return timeStep_;
	}

	// 用于设置默认智能体的属性，以便在创建新智能体时使用这些默认值。
	void RVOSimulator::setAgentDefaults(float neighborDist, size_t maxNeighbors, float timeHorizon, float radius, float maxSpeed, const Vector3 &velocity)
	{
		if (defaultAgent_ == NULL)
		{
			defaultAgent_ = new Agent(this); // 如果defaultAgent_为空，则分配新的Agent对象并将其初始化
		}

		// 设置默认智能体的属性
		defaultAgent_->maxNeighbors_ = maxNeighbors; // 设置最大邻居数
		defaultAgent_->maxSpeed_ = maxSpeed;	     // 设置最大速度
		defaultAgent_->neighborDist_ = neighborDist; // 设置邻居距离
		defaultAgent_->radius_ = radius;	     // 设置半径
		defaultAgent_->timeHorizon_ = timeHorizon;   // 设置时间视野
		defaultAgent_->velocity_ = velocity;	     // 设置初始速度
	}

	void RVOSimulator::setAgentMaxNeighbors(size_t agentNo, size_t maxNeighbors)
	{
		agents_[agentNo]->maxNeighbors_ = maxNeighbors; // 设置指定智能体的最大邻居数
	}

	void RVOSimulator::setAgentMaxSpeed(size_t agentNo, float maxSpeed)
	{
		agents_[agentNo]->maxSpeed_ = maxSpeed; // 设置指定智能体的最大速度
	}

	void RVOSimulator::setAgentNeighborDist(size_t agentNo, float neighborDist)
	{
		agents_[agentNo]->neighborDist_ = neighborDist; // 设置指定智能体的邻居距离
	}

	void RVOSimulator::setAgentPosition(size_t agentNo, const Vector3 &position)
	{
		agents_[agentNo]->position_ = position; // 设置指定智能体的位置
	}

	void RVOSimulator::setAgentPrefVelocity(size_t agentNo, const Vector3 &prefVelocity)
	{
		agents_[agentNo]->prefVelocity_ = prefVelocity; // 设置指定智能体的偏好速度
	}

	void RVOSimulator::setAgentRadius(size_t agentNo, float radius)
	{
		agents_[agentNo]->radius_ = radius; // 设置指定智能体的半径
	}

	void RVOSimulator::setAgentTimeHorizon(size_t agentNo, float timeHorizon)
	{
		agents_[agentNo]->timeHorizon_ = timeHorizon; // 设置指定智能体的时间视野
	}

	void RVOSimulator::setAgentVelocity(size_t agentNo, const Vector3 &velocity)
	{
		agents_[agentNo]->velocity_ = velocity; // 设置指定智能体的速度
	}

	void RVOSimulator::setTimeStep(float timeStep)
	{
		timeStep_ = timeStep; // 设置仿真器的时间步长
	}

}
